/******************************************************************************
*  Nano-RK, a real-time operating system for sensor networks.
*  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
*  All rights reserved.
*
*  This is the Open Source Version of Nano-RK included as part of a Dual
*  Licensing Model. If you are unsure which license to use please refer to:
*  http://www.nanork.org/nano-RK/wiki/Licensing
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation, version 2.0 of the License.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*  Contributing Authors (specific to this file):
*  Anthony Rowe
*  Zane Starr
*  Anand Eswaren
*******************************************************************************/

#include <nrk.h>
#include <nrk_task.h>
#include <include.h>
#include <ulib.h>
#include <nrk_timer.h>
#include <nrk_scheduler.h>
#include <nrk_error.h>
#include <nrk_stack_check.h>
#include <nrk_defs.h>

//#define TIME_PAD  2

inline void _nrk_wait_for_scheduler ();

uint8_t nrk_get_high_ready_task_ID ()
{
    return (_head_node->task_ID);
}

void nrk_print_readyQ ()
{
#ifdef CBS_SUPPORT
    nrk_queue *ptr;
    ptr = _head_node;
    nrk_kprintf (PSTR ("nrk_queue: "));
    while (ptr != NULL)
    {
        // not well tested yet...
        printf ("(%d,%d,%d,%d)", ptr->task_ID, nrk_task_TCB[ptr->task_ID].cpu_remaining, nrk_task_TCB[ptr->task_ID].next_period,nrk_task_TCB[ptr->task_ID].next_wakeup); 
        ptr = ptr->Next;
    }
    printf("%d", _nrk_prev_timer_val);
    nrk_kprintf (PSTR ("\n\r"));
#elif defined(SRP_SUPPORT)
    nrk_time_t print_Q_time;
    nrk_queue *ptr;
    ptr = _head_node;
    nrk_time_get(&print_Q_time);
  	printf ("Q: %u nrk_queue: ", print_Q_time.secs);
    while (ptr != NULL)
    {
        printf ("%d ", ptr->task_ID);
        ptr = ptr->Next;
    }
    printf (" Locks: ");
    for (uint8_t i=0; i<NRK_MAX_RESOURCE_CNT; i++) {
      printf("%u ", current_lock[i]);
    }
    printf(" Ceil: %u \r\n", nrk_get_system_ceiling());
#else 
    nrk_queue *ptr;
    ptr = _head_node;
    nrk_kprintf (PSTR ("nrk_queue: "));
    while (ptr != NULL)
    {
        // not well tested yet...
        printf ("(%d,%d,%d,%d)", ptr->task_ID, nrk_task_TCB[ptr->task_ID].cpu_remaining, nrk_task_TCB[ptr->task_ID].next_period,nrk_task_TCB[ptr->task_ID].next_wakeup); 
        ptr = ptr->Next;
    }
    printf("%d", _nrk_prev_timer_val);
    nrk_kprintf (PSTR ("\n\r"));
#endif
}


void nrk_add_to_readyQ (int8_t task_ID)
{
    nrk_queue *NextNode;
    nrk_queue *CurNode;

    //printf( "nrk_add_to_readyQ %d\n",task_ID );
    // nrk_queue full
    if (_free_node == NULL)
    {
        return;
    }

    NextNode = _head_node;
    CurNode = _free_node;

    // In this section of code, search the queue for the task who you should be
    // inserted in front of
    if (_head_node != NULL)
    {
#ifdef SRP_SUPPORT
      // some debug diagnostics for SRP support
        nrk_time_t c_time;
        nrk_time_get(&c_time);

        if (queue_re_add_task ==1) {
            printf("Task %u has requeue set to 1\r\n", task_ID);
        }

        printf("Add T%u PLvl: %u Ceil: %u , %u s \r\n", task_ID, nrk_get_preemption_lvl(task_ID), nrk_get_system_ceiling(), c_time.secs);
#endif          
          while (NextNode != NULL)
          {
#ifdef SRP_SUPPORT
              if ((NextNode->task_ID == NRK_IDLE_TASK_ID) || 
                  ((nrk_get_preemption_lvl(task_ID) + queue_re_add_task) > nrk_get_preemption_lvl(NextNode->task_ID))) {
                 break;
              }
#elif defined(EDF_SUPPPORT)
              // The list is maintained in order, EDF wants to order by shortest next_period
              if ((NextNode->task_ID == NRK_IDLE_TASK_ID) || nrk_task_TCB[NextNode->task_ID].next_wakeup> nrk_task_TCB[task_ID].next_wakeup) {
                  break;
              }
#else
              // Basic ordering scheme is based on priority
              // Interestingly, if you look at nrk_task.c:
              //    Task priority (0 == highest, 63 == lowest)
              // However, this is reverse, according to the if statements below...
              if (nrk_task_TCB[NextNode->task_ID].elevated_prio_flag)  
                  if (nrk_task_TCB[NextNode->task_ID].task_prio_ceil <
                          nrk_task_TCB[task_ID].task_prio)
                      break;
              if (nrk_task_TCB[task_ID].elevated_prio_flag)
                  if (nrk_task_TCB[NextNode->task_ID].task_prio <
                          nrk_task_TCB[task_ID].task_prio_ceil)
                      break;
              if (nrk_task_TCB[NextNode->task_ID].task_prio <  
                      nrk_task_TCB[task_ID].task_prio)
                  break;
#endif
              NextNode = NextNode->Next;
        }


        //      while ((NextNode != NULL) && ((nrk_task_TCB[NextNode->task_ID].task_prio >= nrk_task_TCB[task_ID].task_prio)|| ) {
        //              NextNode = NextNode->Next;}
        // Stop if nextNode is freenode or next node less prio or (equal and elevated
        // Issues - 1 comes, becomes 2', 1 more comes (2' 1) then 2 comes where should it be placed ?
        // 2' 2  1 or 2 2' 1 in ready q , what happens after 2'->1, what if 2'->2

    }

    CurNode->task_ID = task_ID;
    _free_node = _free_node->Next;

    if (NextNode == _head_node)
    {
        //at start
        if (_head_node != NULL)
        {
            CurNode->Next = _head_node;
            CurNode->Prev = NULL;
            _head_node->Prev = CurNode;
        }
        else
        {
            CurNode->Next = NULL;
            CurNode->Prev = NULL;
            _free_node->Prev = CurNode;
        }
        _head_node = CurNode;

    }
    else
    {
        if (NextNode != _free_node)
        {
            // Insert  in middle

            CurNode->Prev = NextNode->Prev;
            CurNode->Next = NextNode;
            (NextNode->Prev)->Next = CurNode;
            NextNode->Prev = CurNode;
        }
        else
        {
            //insert at end
            CurNode->Next = NULL;
            CurNode->Prev = _free_node->Prev;
            _free_node->Prev = CurNode;
        }

    }

#ifdef ARRIVAL_TICKS
    // instead of counting cnts, why not just get the actual time!
    nrk_time_t tt;
    nrk_time_get(&tt);
    task_arrival_time[task_ID] = tt.secs;
#endif
}


void nrk_rem_from_readyQ (int8_t task_ID)
{

    nrk_queue *CurNode;
//      nrk_queue       *tempNode;

//return;

    /*CurNode = _head_node;

       _head_node = _head_node->Next;
       _head_node->Prev = NULL;

       if (_free_node == NULL)
       {
       tempNode = _head_node;
       while (tempNode->Next!=NULL) tempNode=tempNode->Next;
       CurNode->Next = NULL;
       CurNode->Prev = tempNode;
       tempNode->Next = CurNode;
       _free_node = CurNode;
       }
       else
       {
       CurNode->Next = _free_node;
       _free_node->Prev = CurNode;
       _free_node = CurNode;
       }
     */

//      printf("nrk_rem_from_readyQ_nrk_queue %d\n",task_ID);

    if (_head_node == NULL)
        return;

    CurNode = _head_node;

    if (_head_node->task_ID == task_ID)
    {
        //REmove from start
        _head_node = _head_node->Next;
        _head_node->Prev = NULL;
    }
    else
    {
        while ((CurNode != NULL) && (CurNode->task_ID != task_ID))
            CurNode = CurNode->Next;
        if (CurNode == NULL)
            return;


        (CurNode->Prev)->Next = CurNode->Next;      //Both for middle and end
        if (CurNode->Next != NULL)
            (CurNode->Next)->Prev = CurNode->Prev;    // Only for middle

    }



    // Add to free list
    if (_free_node == NULL)
    {
        _free_node = CurNode;
        _free_node->Next = NULL;
    }
    else
    {
        CurNode->Next = _free_node;
        _free_node->Prev = CurNode;
        _free_node = CurNode;
    }
    _free_node->Prev = NULL;
}





nrk_status_t nrk_activate_task (nrk_task_type * Task)
{
    uint8_t rtype;
    void *topOfStackPtr;

    topOfStackPtr =
        (void *) nrk_task_stk_init (Task->task, Task->Ptos, Task->Pbos);

    //printf("activate %d\n",(int)Task.task_ID);
    if (Task->FirstActivation == TRUE)
    {
        rtype = nrk_TCB_init (Task, topOfStackPtr, Task->Pbos, 0, (void *) 0, 0);
        Task->FirstActivation = FALSE;

    }
    else
    {
        if (nrk_task_TCB[Task->task_ID].task_state != SUSPENDED)
            return NRK_ERROR;
        //Re-init some parts of TCB

        nrk_task_TCB[Task->task_ID].OSTaskStkPtr = (NRK_STK *) topOfStackPtr;



    }

#ifdef SRP_SUPPORT
    uint8_t i;
    uint8_t sem_id;
    nrk_sem_t *current_sem;

    // Whenever we activate a task, we need to see which resources it uses, and 
    // adjust the ceiling.
    for(i=0;i<nrk_task_TCB[Task->task_ID].num_sems;i++) {
        //current_sem = nrk_task_TCB[Task->task_ID].sems[i];
        sem_id = nrk_get_resource_index(nrk_task_TCB[Task->task_ID].sems[i]);
        if ( nrk_task_TCB[Task->task_ID].period < nrk_sem_list[sem_id].shortest_period) {
            printf("Resource %u's shortest period: %u -> %u\r\n", sem_id, nrk_sem_list[sem_id].shortest_period, nrk_task_TCB[Task->task_ID].period);
            nrk_sem_list[sem_id].shortest_period= nrk_task_TCB[Task->task_ID].period;
        }
    }

    if ( nrk_task_TCB[Task->task_ID].period > max_period ) {
      max_period = nrk_task_TCB[Task->task_ID].period;
    }
#endif

    //nrk_task_TCB[Task->task_ID].task_state = READY;

    // Remove from suspended or waiting if extended

    // OSSchedLock();


    // If Idle Task then Add to ready Q
    //if(Task->task_ID==0) nrk_add_to_readyQ(Task->task_ID);
    //nrk_add_to_readyQ(Task->task_ID);
    //printf( "task %d nw %d \r\n",Task->task_ID,nrk_task_TCB[Task->task_ID].next_wakeup);
    //printf( "task %d nw %d \r\n",Task->task_ID,Task->offset.secs);
    /*
    if (nrk_task_TCB[Task->task_ID].next_wakeup == 0)
    {
        nrk_task_TCB[Task->task_ID].task_state = READY;
        nrk_add_to_readyQ (Task->task_ID);
    }
    */

    return NRK_OK;
}



nrk_status_t nrk_terminate_task ()
{
    nrk_rem_from_readyQ (nrk_cur_task_TCB->task_ID);
    nrk_cur_task_TCB->task_state = FINISHED;

    // HAHA, there is NO next period...
    nrk_wait_until_next_period ();
    return NRK_OK;
}

int8_t nrk_wait_until_next_period ()
{
    uint8_t timer;

    nrk_stack_check ();
// Next Period Wakeup Time is Set inside scheduler when a task becomes Runnable
    nrk_int_disable ();
    nrk_cur_task_TCB->num_periods = 1;
    nrk_cur_task_TCB->suspend_flag = 1;
    timer = _nrk_os_timer_get ();

//nrk_cur_task_TCB->cpu_remaining=_nrk_prev_timer_val+1;
/*
    for (uint8_t i = 0; i < NRK_MAX_TASKS; i++) {
        nrk_task_TCB[i].CASH_flag = 0;
    }
*/

    if (timer < (MAX_SCHED_WAKEUP_TIME - TIME_PAD))
        if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ())
        {
            timer += TIME_PAD;
            _nrk_prev_timer_val = timer;                  // pdiener: why is this only set in this special case?
            _nrk_set_next_wakeup (timer);                 // pdiener: Set next wakeup to NOW
        }

    nrk_int_enable ();
    _nrk_wait_for_scheduler ();
    return NRK_OK;
}

int8_t nrk_wait_until_next_n_periods (uint16_t p)
{
    uint8_t timer;

    nrk_stack_check ();

    if (p == 0)
        p = 1;
// Next Period Wakeup Time is Set inside scheduler when a task becomes Runnable
    nrk_int_disable ();
    nrk_cur_task_TCB->suspend_flag = 1;
    nrk_cur_task_TCB->num_periods = p;
    timer = _nrk_os_timer_get ();

//nrk_cur_task_TCB->cpu_remaining=_nrk_prev_timer_val+1;

// +2 allows for potential time conflict resolution
    if (timer < (MAX_SCHED_WAKEUP_TIME - TIME_PAD))       // 254 8bit overflow point - 2
        if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ())
        {
            timer += TIME_PAD;
            _nrk_prev_timer_val = timer;
            _nrk_set_next_wakeup (timer);                 // pdiener: Set next wakeup to NOW
        }

    nrk_int_enable ();
    _nrk_wait_for_scheduler ();
    return NRK_OK;
}

/*
 * nrk_wait_ticks()
 *
 * This function will wait until a specified number of
 * timer ticks after the curret OS tick timer.
 *
 */

int8_t nrk_wait_ticks (uint16_t ticks)
{
    uint8_t timer;
    nrk_int_disable ();
    nrk_cur_task_TCB->suspend_flag = 1;
    timer = _nrk_os_timer_get ();
    nrk_cur_task_TCB->next_wakeup = ticks + timer;

    if (timer < MAX_SCHED_WAKEUP_TIME - TIME_PAD)
        if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ())
        {
            timer += TIME_PAD;
            _nrk_prev_timer_val = timer;
            _nrk_set_next_wakeup (timer);
        }
//else nrk_cur_task_TCB->next_wakeup=ticks+1;
    nrk_int_enable ();
//while(nrk_cur_task_TCB->suspend_flag==1);
    _nrk_wait_for_scheduler ();
    return NRK_OK;
}


/*
 * nrk_wait_until_ticks()
 *
 * This function will wait until a specified number of
 * timer ticks starting from when the task was swapped in.
 * This means that this function can set periodic timing
 * taking into account any task processing time.
 *
 */

int8_t nrk_wait_until_ticks (uint16_t ticks)
{
    uint8_t timer;
    nrk_int_disable ();
    nrk_cur_task_TCB->suspend_flag = 1;
    nrk_cur_task_TCB->next_wakeup = ticks;
    // printf( "t %u\r\n",ticks );
    timer = _nrk_os_timer_get ();

    if (timer < MAX_SCHED_WAKEUP_TIME - TIME_PAD)
        if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ())
        {
            timer += TIME_PAD;
            _nrk_prev_timer_val = timer;
            _nrk_set_next_wakeup (timer);
        }
//else nrk_cur_task_TCB->next_wakeup=ticks+1;
    nrk_int_enable ();
//while(nrk_cur_task_TCB->suspend_flag==1);
    _nrk_wait_for_scheduler ();
    return NRK_OK;
}

int8_t nrk_set_next_wakeup (nrk_time_t t)
{
    uint8_t timer;
    uint16_t nw;
    nrk_int_disable ();
    timer = _nrk_os_timer_get ();
    nw = _nrk_time_to_ticks (&t);
    if (nw <= TIME_PAD)
        return NRK_ERROR;
    nrk_cur_task_TCB->next_wakeup = nw + timer;
    /*	if(timer<(254-TIME_PAD))
    		if((timer+TIME_PAD)<=_nrk_get_next_wakeup())
    		{
    			timer+=TIME_PAD;
    			_nrk_prev_timer_val=timer;
    			_nrk_set_next_wakeup(timer);
    		}
    */
//      nrk_cur_task_TCB->nw_flag=1;
    nrk_int_enable ();

    return NRK_OK;
}

/*
 * nrk_wait_until_nw()
 *
 * This function will wait until a specified number of
 * timer ticks starting from when the task was swapped in.
 * This means that this function can set periodic timing
 * taking into account any task processing time.
 *
 */

int8_t nrk_wait_until_nw ()
{
    uint8_t timer;
    nrk_int_disable ();
    nrk_cur_task_TCB->suspend_flag = 1;
    nrk_cur_task_TCB->nw_flag = 1;
    timer = _nrk_os_timer_get ();

    if (timer < MAX_SCHED_WAKEUP_TIME - TIME_PAD)
        if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ())
        {
            timer += TIME_PAD;
            _nrk_prev_timer_val = timer;
            _nrk_set_next_wakeup (timer);
        }
//else nrk_cur_task_TCB->next_wakeup=ticks+1;
    nrk_int_enable ();
//while(nrk_cur_task_TCB->suspend_flag==1);
    _nrk_wait_for_scheduler ();
    return NRK_OK;
}


int8_t nrk_wait (nrk_time_t t)
{
    uint8_t timer;
    uint16_t nw;

    nrk_stack_check ();

    nrk_int_disable ();
    nrk_cur_task_TCB->suspend_flag = 1;
    nrk_cur_task_TCB->num_periods = 1;
    timer = _nrk_os_timer_get ();

//printf( "t1 %lu %lu\n",t.secs, t.nano_secs/NANOS_PER_MS);

    nw = _nrk_time_to_ticks (&t);
// printf( "t2 %u %u\r\n",timer, nw);
    nrk_cur_task_TCB->next_wakeup = nw + timer;
//printf( "wu %u\n",nrk_cur_task_TCB->next_wakeup );
    if (timer < (MAX_SCHED_WAKEUP_TIME - TIME_PAD))
    {
        if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ())
        {
            timer += TIME_PAD;
            _nrk_prev_timer_val = timer;
            _nrk_set_next_wakeup (timer);
        }
    }
    nrk_int_enable ();

    _nrk_wait_for_scheduler ();
    return NRK_OK;
}


inline void _nrk_wait_for_scheduler ()
{

    //TIMSK = BM (OCIE1A);
    do
    {
        nrk_idle ();                // wait for the interrupt to tick... // pdiener: halt CPU here until any interrupt triggers
    }
    while ((volatile uint8_t) nrk_cur_task_TCB->suspend_flag == 1); // pdiener: make shure that was the right interrupt

    //TIMSK = BM (OCIE1A) | BM(OCIE0);
}


int8_t nrk_wait_until (nrk_time_t t)
{
    nrk_time_t ct;
    int8_t v;
//    uint8_t c;

    //c = _nrk_os_timer_get ();
    //do{
    //}while(_nrk_os_timer_get()==c);

    //ttt=c+1;
    nrk_time_get (&ct);

    v = nrk_time_sub (&t, t, ct);
    //nrk_time_compact_nanos(&t);
    if (v == NRK_ERROR)
    {
        return NRK_ERROR;
    }
//if(t.secs<ct.secs) return 0;
//if(t.secs==ct.secs && t.nano_secs<ct.nano_secs) return 0;

//t.secs-=ct.secs;
//t.nano_secs-=ct.nano_secs;
//
    nrk_wait (t);

    return NRK_OK;
}


uint8_t nrk_get_pid ()
{
    return nrk_cur_task_TCB->task_ID;
}

#ifdef SRP_SUPPORT
// Helper function to get the system ceiling
uint16_t nrk_get_system_ceiling(void) {
  uint8_t i;
  uint16_t sys_ceil = 0;
  uint16_t rsrc_ceil;

  for (i = 0; i < NRK_MAX_RESOURCE_CNT; i++) {
     rsrc_ceil = max_period - nrk_sem_list[i].shortest_period;
     if((nrk_sem_list[i].value == 0) && (rsrc_ceil > sys_ceil)) {
       sys_ceil = rsrc_ceil;
     }
  }

  return sys_ceil;
}


uint16_t nrk_get_preemption_lvl(uint8_t task_ID) {
    uint8_t i;
    uint16_t shortest_period = nrk_task_TCB[task_ID].next_period; // The shortest period possible should equal your period

    // get the shortest period from all the semaphores...
    for (i=0; i < NRK_MAX_RESOURCE_CNT; i++) {
        if (nrk_sem_list[i].task_ID == task_ID) {
            if (nrk_sem_list[i].shortest_period < shortest_period) {
                shortest_period = nrk_sem_list[i].shortest_period;
            }
        }
    }

    return max_period - shortest_period;
}
#endif

